//
// Created by PulsarV on 18-5-9.
//

#include <rc_move/rcmove.h>
#include <cstring>
#include <rc_gpio/c_gpio.h>
#include <rc_gpio/soft_pwm.h>
#include <mraa/common.hpp>
#include <mraa/gpio.hpp>
#include <mraa/pwm.hpp>
#include <rc_log/slog.hpp>
#include <utility>

namespace rccore {
    namespace move {
        void init_pwm(const std::shared_ptr<common::Context> &pcontext) {
            RC::GPIO::pwm_begin(pcontext->pconfig->pconfigInfo->PWM_1);
            RC::GPIO::pwm_set_frequency(pcontext->pconfig->pconfigInfo->PWM_1, 3413333);
            RC::GPIO::pwm_set_duty_cycle(pcontext->pconfig->pconfigInfo->PWM_1, 1706667);
            RC::GPIO::pwm_start(pcontext->pconfig->pconfigInfo->PWM_1);

            RC::GPIO::pwm_begin(pcontext->pconfig->pconfigInfo->PWM_2);
            RC::GPIO::pwm_set_frequency(pcontext->pconfig->pconfigInfo->PWM_2, 3413333);
            RC::GPIO::pwm_set_duty_cycle(pcontext->pconfig->pconfigInfo->PWM_2, 1706667);
            RC::GPIO::pwm_start(pcontext->pconfig->pconfigInfo->PWM_2);

            RC::GPIO::pwm_begin(pcontext->pconfig->pconfigInfo->PWM_3);
            RC::GPIO::pwm_set_frequency(pcontext->pconfig->pconfigInfo->PWM_3, 3413333);
            RC::GPIO::pwm_set_duty_cycle(pcontext->pconfig->pconfigInfo->PWM_3, 1706667);
            RC::GPIO::pwm_start(pcontext->pconfig->pconfigInfo->PWM_3);

            RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_1);
            RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_2);
            RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_3);
        }

        void init_direction(const std::shared_ptr<common::Context> &pcontext) {
            RC::GPIO::pinMode(pcontext->pconfig->pconfigInfo->GPIO_1, RC_GPIO_OUTPUT);
            RC::GPIO::digitalWrite(pcontext->pconfig->pconfigInfo->GPIO_1, RC_GPIO_LOW);

            RC::GPIO::pinMode(pcontext->pconfig->pconfigInfo->GPIO_2, RC_GPIO_OUTPUT);
            RC::GPIO::digitalWrite(pcontext->pconfig->pconfigInfo->GPIO_2, RC_GPIO_LOW);

            RC::GPIO::pinMode(pcontext->pconfig->pconfigInfo->GPIO_3, RC_GPIO_OUTPUT);
            RC::GPIO::digitalWrite(pcontext->pconfig->pconfigInfo->GPIO_3, RC_GPIO_LOW);
        }

        void release_all(std::shared_ptr<common::Context> pcontext) {
            RC::GPIO::pinFree(pcontext->pconfig->pconfigInfo->GPIO_1);
            RC::GPIO::pinFree(pcontext->pconfig->pconfigInfo->GPIO_2);
            RC::GPIO::pinFree(pcontext->pconfig->pconfigInfo->GPIO_3);

            RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_1);
            RC::GPIO::pwm_free(pcontext->pconfig->pconfigInfo->PWM_1);

            RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_2);
            RC::GPIO::pwm_free(pcontext->pconfig->pconfigInfo->PWM_2);

            RC::GPIO::pwm_stop(pcontext->pconfig->pconfigInfo->PWM_3);
            RC::GPIO::pwm_free(pcontext->pconfig->pconfigInfo->PWM_3);
        }

        void RobotCarMove::dir_pin(std::shared_ptr<mraa::Gpio> pgpio) {
            mraa::Result status = pgpio->dir(mraa::DIR_OUT);
            if (status != mraa::SUCCESS) {
                printError(status);
            }
        }

        void RobotCarMove::set_value(std::shared_ptr<mraa::Gpio> pgpio, int value) {
            mraa::Result status = pgpio->write(value);
            if (status != mraa::SUCCESS) {
                printError(status);
            }
        }

        void RobotCarMove::init() {
#ifdef USE_DSP_DEVICE
            if(moveDevice.serial_device!=""){
                serial_device.bind_port(moveDevice.serial_device,9600);
                if(serial_device.is_open()){
                    slog::success<<"控制器串口打开成功"<<slog::endl;
                } else{
                    slog::err<<"控制器串口打开失败"<<slog::endl;
                }
            }
#else
            if (pcontext->pconfig->pconfigInfo->USE_MRAA) {
                _pwm_1.reset(new mraa::Pwm(pcontext->pconfig->pconfigInfo->PWM_1));
                _pwm_1->enable(true);
                _pwm_2.reset(new mraa::Pwm(pcontext->pconfig->pconfigInfo->PWM_2));
                _pwm_2->enable(true);
                _pwm_3.reset(new mraa::Pwm(pcontext->pconfig->pconfigInfo->PWM_3));
                _pwm_3->enable(true);
                _gpio_1.reset(new mraa::Gpio(pcontext->pconfig->pconfigInfo->GPIO_1));
                dir_pin(_gpio_1);
                _gpio_2.reset(new mraa::Gpio(pcontext->pconfig->pconfigInfo->GPIO_2));
                dir_pin(_gpio_2);
                _gpio_3.reset(new mraa::Gpio(pcontext->pconfig->pconfigInfo->GPIO_3));
                dir_pin(_gpio_3);
            } else {
                init_pwm(pcontext);
                init_direction(pcontext);
            }
#endif // USE_DSP_DEVICE
        }


        int start() {
//    std::fstream mapped;
//    std::ifstream map;

//        cv::VideoCapture cap;
//        switch (this->type) {
//            case RC_PLAY_BY_CAMERA:
//                if (this->camera_id != -1) {
//                    slog::info << "Open Camera " << this->camera_id << " From Device" << slog::endl;
//                    try {
//                        cap.open(this->camera_id);
//                    } catch (cv::Exception &e) {
//                        slog::info << "Open Camera " << this->camera_id << " Error" << slog::endl;
//                        return -1;
//                    }
//
//
//                } else {
//                    slog::info << RC_MOVE_DEVICE_PORT_INITATION_ERROR << slog::endl;
//                    return -1;
//                }
//
//                break;
//            case RC_PLAY_BY_VIDEO:
//                if (this->video != nullptr) {
//                    LOG::logInfo((char *) "Open Camera Device From Files");
//                    cap.open(this->camera_id);
//                } else {
//                    slog::err << RC_MOVE_DEVICE_PORT_INITATION_ERROR << slog::endl;
//                    return -1;
//
//                }
//                break;
//        }

//    if (this->mapping != NULL) {
//        map.open(this->mapping);
//    } else {
//        mapped.open("map.bin", std::ios::app);
//    }

//        if (cap.isOpened()) {
//            LOG::logSuccess((char *) "Open Camera Device Successed");
//            CV::BodyDetceter decter;
//            int load_cascade = decter.init_body_cascade(RC_BODY_CASCADES_FILE_PATH);
//            while (true) {
//                cv::Mat frame, output;
//                cap >> frame;
//                cv::Mat re_frame;
//                cv::resize(frame, re_frame, cv::Size(128, 128), 0, 0, cv::INTER_LINEAR);
//
//                if (not frame.empty()) {
//                    //巡线
//                    if (this->AutoMove) {
//                        //TODO:寻线
//                        int ans[2] = {0, 0};
//                        cv::Mat thresh_image;
//                        cv::Mat gray_image;
//                        CV::detcetByRightAndLeft(re_frame, ans);
//                        if (this->serial_device->is_opend()) {
//                            if (ans[0] > (128 / 2) + 10) {
//                                this->wheel_AC();
//                            }
//                            if (ans[0] < (128 / 2) - 10) {
//                                this->wheel_CW();
//                            }
//                            this->wheel_go_backward();
//                            char buffer[64] = {'\0'};
//                            this->serial_device->recive(buffer, 64);
//                            std::string data = buffer;
//                        }
//                    }
//                    //自动跟随
//                    if (this->AutoFollow && (load_cascade == 1)) {
//                        cv::Point center, img_center;
//                        img_center.x = frame.cols / 2;
//                        img_center.y = frame.rows / 2;
//                        std::vector <cv::Rect> body = decter.detcetBody(frame);
//                        if (not body.empty()) {
//                            center.x = body[0].x + cvRound(body[0].width / 2.0);
//                            center.y = body[0].y + cvRound(body[0].height / 2.0);
//                            cv::circle(frame, center, 3, cv::Scalar(0, 0, 255), -1);
//                        }
//                        cv::circle(frame, img_center, 10, cv::Scalar(255, 0, 0), -1);
//                        if (this->serial_device->is_opend()) {
//                            //TODO：转向修正
//                        }
//                    }
//                }
//
//                cv::imshow("Origin Output", frame);
//                char key = cv::waitKey(20);
//                switch (key) {
//                    case 'c':
//                        LOG::logInfo((char *) "Model Change to AutoMove");
//                        this->AutoMove = this->AutoMove == true ? false : true;
//                        this->AutoFollow = false;
//                        break;
//                    case 'f':
//                        LOG::logInfo((char *) "Model Change to AutoFllow");
//                        this->AutoFollow = this->AutoFollow == true ? false : true;
//                        this->AutoMove = false;
//                        break;
//                    default:
//                        break;
//                }
////                if (map.is_open()) {
////                    if (!map.eof()) {
////                        char com = map.get();
////                        this->command(com);
////                        continue;
////                    }
////                }
//                if (key == 'x')break;
//
////                if (mapping == NULL)
////                    if ((int) key > 65 and (int) key < 122)
////                        mapped << &key;
//                if (this->serial_device->is_opend()) {
//                    this->command(key);
//                }
////            }
//            }
//        } else
//            return LOG::logError(RC_OPEN_CAMERA_ERROR);
//        cv::destroyAllWindows();
//    mapped.close();
            return 1;
        }

        void RobotCarMove::command(char com, float speed_1, float speed_2, float speed_3) {
            switch (com) {
                case 'a':
                    this->wheel_AC(speed_1, speed_2, speed_3);
                    break;
                case 'd':
                    this->wheel_CW(speed_1, speed_2, speed_3);
                    break;
                case 'w':
                    this->wheel_go_forward(speed_1, speed_2);
                    break;
                case 's':
                    this->wheel_go_backward(speed_1, speed_2);
                    break;
                case 'r':
                    this->wheel_stop();
                    break;
                default:
                    break;
            }
        }

        char *RobotCarMove::decode(
                bool weel_1_direction, unsigned short wheel_1_speed,
                bool weel_2_direction, unsigned short wheel_2_speed,
                bool weel_3_direction, unsigned short wheel_3_speed) {

            WHEEL_DATA wheelData;
            wheelData.weel_1_direction = weel_1_direction;
            wheelData.weel_2_direction = weel_2_direction;
            wheelData.weel_3_direction = weel_3_direction;
            wheelData.wheel_1_v_m_s = wheel_1_speed;
            wheelData.wheel_2_v_m_s = wheel_2_speed;
            wheelData.wheel_3_v_m_s = wheel_3_speed;
            char *buffer = new char[sizeof(WHEEL_DATA)];
            memcpy(buffer, &wheelData, sizeof(WHEEL_DATA));
            return buffer;
        }

        RobotCarMove::~RobotCarMove() {
            release_all(this->pcontext);
        }


        RobotCarMove::RobotCarMove(std::shared_ptr<common::Context> pcontext) : BaseTask(std::move(pcontext)) {

        }


    }
}
